/**
*	\file
*	\brief Control module for Peggy. Creates animations and graphics for Peggy as well as playing audio via MP3 Player Shiled from Sparkfun.
*	Receives alerts from Wally the sentry turret, and takes action as appropriate
*
*	MegaMan is an Arduiono MEGA 2560 which serves as the main master module in the Peggy-MegaMan-Wally trio.
*	It sends data to be displayed to Peggy (raw framebuffer as well as commands for Peggy's builtin functions).
*	MegaMan also listens (via XBee wireless) to alerts from Wally if Wally has detected movement, and initiates
*	apropriate action (sound and visual alerts) if an "intruder" is found.
*
*	MegaMan has a Sparkfun MP3 Player Shield attached,  as well as a series 1 XBee to talk to Wally (and a log server).
*	It communicates with Peggy over I2C.
*
*	\copyright Copyright 2012 Ole Jakob Skjelten.  All rights reserved.
*
*    This program is free software: you can redistribute it and/or modify
*    it under the terms of the GNU General Public License as published by
*    the Free Software Foundation, either version 3 of the License, or
*    (at your option) any later version.
*
*    This program is distributed in the hope that it will be useful,
*    but WITHOUT ANY WARRANTY; without even the implied warranty of
*    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
*    GNU General Public License for more details.
*
*    You should have received a copy of the GNU General Public License
*    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*
*	\addtogroup MegaMan
*	\{
*/

#include <Wire.h>						// I2C and serial communication
#include <SPI.h>						// SPI communication
#include <SdFat.h>						// add the SdFat Libraries
#include <SdFatUtil.h>
#include <SFEMP3Shield.h>				// add the MP3 Shield Library

#include "MegaMan.h"
#include "flash_pics.h"
#include "p2anim.h"

SFEMP3Shield MP3player;					///< MP3 player object

#define PEGGY_ADDRESS 34				///< Peggy 2.0's address on the I2C bus
#define TWI_FREQ 300000					///< I2C/TWO bus frequency, 300'000 bits/second

const uint8_t speakerVolume = 40;				///< audio volume, where 40 is minimum and 0 maximum volume

uint8_t header[6] = { 0xde,0xad,0xbe,0xef,1,0};		///< 0xdeadbeef is the initialization string of the Peggy 2.0. Once it gets this it will read data from I2C directly to the local framebuffer \note If we want to send commands to Peggy we have to change the last byte to 1
uint8_t cmdHeader[7] = { 0xde,0xad,0xbe,0xef,1,1};	///< Header which gives Peggy the command in the next byte


uint8_t globalCounter;					///<  global counter used to keep persisten values between function calls
boolean globalFlipFlop = 1;				///<  global flag used to keep persisten values between function calls

// Timer interrupt variable definitions (defined static so they will retain value between function calls)
static const uint16_t tics_per_msec     = ((F_CPU / 1000) / 8);	///< Part of our timer interpt (16000000 / 1000 / 8 = 2000
static volatile uint16_t time_sec;		///< time in seconds (volatile)
static volatile uint16_t time_msec;		///< time in milliseconds (volatile)
static volatile uint8_t heartBeatCounter;	///< counting heartbeats \note Redundant?

// XBee communication variables
boolean abortFlag;				///< Set in timer interrupt when XBee message received. Checked and acted upon in main loop every iteration.
uint8_t sender = 9;				///< The sender who's contacting us
uint8_t cmd = 9;				///< The sender's requested command
char *cmdParam;					///< The sender's command parameters

/** 	\fn ISR (TIMER5_COMPA_vect)
*	\brief Timer interrupt
*	Used primarily to check that the UART has been quiet for a while before whitching to our own
*	graphics after having done UART-SPI conversion
*	\param TIMER5_COMPA_vect We use TIMER5 on this Arduino MEGA 2560 to avoid collisions with built-in Arduino libraries using TIMER1-3
*/
ISR (TIMER5_COMPA_vect)	// We use TIMER5 on this Arduino MEGA 2560 to avoid collisions with built-in Arduino libraries using TIMER1-3
{						// We probably do not need msec, reduce frequency - we only need 1 second or 0.1 seconds
	time_msec++;
	if (time_msec >= 1000) {
		time_msec = 0;
		time_sec++;
		heartBeatCounter++;
		if(heartBeatCounter == 1)				// Triggering every second
			{
			//Serial1.println("{s0:c0:ALIVE}");	// Sending heartbeat every 10 seconds
			//Serial.print(time_sec);
			//Serial.println(" - Sending heartbeat on XBee");
			if (Serial1.available() && processXbeeMsg()) abortFlag = 1;
			heartBeatCounter = 0;
			}

	}  
}

/*
*	Raw dump of serial data from video processing sketch \note Broken now?
*/
void videoTransfer(uint8_t count)
{
	//uint8_t count = Serial.available();	// How many character on the UART input buffer...

	if (count > 0) 			// ...more than one, so we start reading the UART
		{					// dont allow send too many bytes at once, dont want to exceed the buffer
							// size of the Wire library
			if (count > 16) count = 16;

			Wire.beginTransmission(PEGGY_ADDRESS);
			while (count-- > 0 )
			{
				uint8_t c = Serial.read();
				Wire.write(c);
			}
			Wire.endTransmission();

	}
}

/*
*	This function takes an image and copies to the frameBuffer.
*	NOTE: The least significan bits and most significant bits are swaped on Peggy 2.0!
*	This means that decimal 15 (b00001111) will light up the LEFT LED, and keep the right LED off
*	When processing this we need to swap nibbles}
*	NOTE: Very inefficient - we should write data directly without using the frameBuffer
*/
void imageFromFlash(unsigned char *picture)
{
	// Get image from flash
	uint16_t i = 0;
	for (i = 0; i < 325; i++) {
		frameBuffer[i] = pgm_read_byte_near(picture + i);
		}
		sendBuffer(&frameBuffer[0]);	// Start transmitting the frameBuffer to Peggy 2.0 over the I2C bus
}



/*
*	Takes a pixel pos (0..24, 0..24) and a gradient (0..15) and sends to Peggy 2.0
*	NOTE:  Needs optimization - very inefficient
*/
void putPixel(uint8_t x, uint8_t y, uint8_t gradient)
{
    // We need to change from x pixels, to x_byte byte;
    //	Bytes: 	 	 0	  1	   2  ....
    //  Pixels:		0|1  2|3  4|5 ....

    int pos;
    if (x % 2 == 0) 													// We have an *even* x coordinate for the pixel - i.e. leftmost in the byte (MSB)
    {
     	pos = (x / 2) + (y * 13);										// Convert pixel pos to byte number (for the row)
     	setLeftN(frameBuffer[pos], gradient);							// Inserting nibble into leftmost part of byte (MACRO)


    } else					 											// We have an odd number of pixels
    {
		x--;															// We subtract one to get the right nibble address (beginning of the byte we need)
		pos = (x / 2) + (y * 13);										// Convert pixel pos to byte number (for the row)
		setRightN(frameBuffer[pos], gradient);							// Inserting nibble into rightmost part of byte (MACRO)
    }

	sendBuffer(frameBuffer);	// Start transmitting the frameBuffer to Peggy 2.0 over the I2C bus
}



/**
*	\brief This function fills the frameBuffer with some pattern/graphic/text and finally calls the sendBuffer function to transmit the whole buffer to Peggy2
*
*	\param char update[] text string describing the update we want.
*		"testpattern"	- draw pattern of straight lines that fade in and out
*		"clear"		- turns all LEDs off
*		"allon"		- turns all LEDs on
*		"scan"		- radar scan animation
*		"skull"		- skull and crossbones animation
*
*/
void updatePeggyDisplay(char update[])
{

	uint8_t update_nr = 0;

	if (!strcmp(update, "testpattern")) update_nr = 1;
	if (!strcmp(update, "clear"))  update_nr = 2;
	if (!strcmp(update, "allon"))  update_nr = 3;
	if (!strcmp(update, "scan"))  update_nr = 4;
	if (!strcmp(update, "skull"))  update_nr = 5;
	if (!strcmp(update, "waiting"))  update_nr = 6;
	if (!strcmp(update, "sound"))  update_nr = 7;
	if (!strcmp(update, "stars"))  update_nr = 8;


	switch(update_nr)
	{
		case 1:	{	// Draw testpattern ("checkerboard" 01010101... t)

			uint8_t xres = 13;
			uint8_t yres = 25;

			uint8_t *ptr = frameBuffer;	// ptr now points to the frameBuffer array

			uint8_t valFirst;
			uint8_t valSecond;
			valFirst = globalCounter;	// Need a global var to keep track of LED strengths between calls
			globalCounter = globalCounter + globalFlipFlop;
			if (globalCounter == 15) globalFlipFlop = -1;
			if (globalCounter == 0) globalFlipFlop = 1;
			valSecond = 0; 				// Every second row always off

			for (uint8_t y =0; y < yres; y++)
			{
				for (uint8_t  x = 0; x < xres; x++)
				{
					*ptr++ = valFirst | (valSecond << 4);
    			}
			}
		}

			sendBuffer(&frameBuffer[0]);	// Start transmitting the frameBuffer to Peggy 2.0 over the I2C bus
			break;

		case 2: // All LEDs off (clear screen)
			Wire.beginTransmission(PEGGY_ADDRESS);
			Wire.write(cmdHeader,6);
			Wire.write(0x01);
			Wire.endTransmission();
			break;

		case 3: // All LEDs on
			memset(&frameBuffer[0], 255, 13*25);
			sendBuffer(&frameBuffer[0]);	// Start transmitting the frameBuffer to Peggy 2.0 over the I2C bus
			break;
		case 4: // Show radar scan
			Wire.beginTransmission(PEGGY_ADDRESS);
			Wire.write(cmdHeader,6);
			Wire.write(0x03);
			Wire.endTransmission();
			break;
		case 5: // Show skull animation
			Wire.beginTransmission(PEGGY_ADDRESS);
			Wire.write(cmdHeader,6);
			Wire.write(0x02);
			Wire.endTransmission();
			break;
		case 6: // Show waiting animation
			Wire.beginTransmission(PEGGY_ADDRESS);
			Wire.write(cmdHeader,6);
			Wire.write(0x04);
			Wire.endTransmission();
			break;
		case 7: // Show sound animation
			Wire.beginTransmission(PEGGY_ADDRESS);
			Wire.write(cmdHeader,6);
			Wire.write(0x05);
			Wire.endTransmission();
			break;
		case 8: // Show starfield animation
			Wire.beginTransmission(PEGGY_ADDRESS);
			Wire.write(cmdHeader,6);
			Wire.write(0x06);
			Wire.endTransmission();
			break;
		default:
			Serial.println("Invalid update requested");

	}

}

/*	This function manipulates the frameBuffer with some effect and finally calls
	the sendBuffer function to transmit the whole buffer to Peggy2
	Valid parameters:
		"fall_down"		- the picture currently on the Peggy 2.0 "falls off" the screen
		"fade_out"		- the picture currently on the Peggy 2.0 fades to black
		"fade_in"		- the picture from flash is being faded in to the buffer from black
*/
void alterBuffer(char effect[], unsigned char *picture) {

	uint8_t effect_nr = 0;

	if (!strcmp(effect, "fall_down")) effect_nr = 1;
	if (!strcmp(effect, "fade_out"))  effect_nr = 2;
	if (!strcmp(effect, "fade_in"))  effect_nr = 3;

	switch(effect_nr) {
		case 1:	{	// Make current frameBuffer "fall out fo the screen" (scroll it down and add 0 to the top)
			for (uint8_t i = 0; i < 25; i++) {	// We need to do it 25 times to make sure we scroll out everything
				for (uint8_t j = 25; j > 0; j--) {
					//memcpy(&frameBuffer[0], &frameBuffer[13], 13*25);
					memcpy(&frameBuffer[13*(j)], &frameBuffer[13*(j-1)], 13);
				}
				// we now need to fill an extra empty line from the top
				memset(&frameBuffer[0], 0, 13);
				sendBuffer(&frameBuffer[0]);	// Start transmitting the frameBuffer to Peggy 2.0 over the I2C bus
				delay(100);	// Just to see what we're doing
				}
			}
			break;

		case 2: {		// Fade out to black
			uint8_t xres = 13;
			uint8_t yres = 25;
			uint8_t leftmost = 0;
			uint8_t rightmost = 0;
			for(uint8_t gradient = 0; gradient < 16; gradient++) {
				for (uint8_t y =0; y < yres; y++) {
					for (uint8_t  x = 0; x < xres; x++) {
						// Reduce value of each 4bit nibble by one
						leftmost = frameBuffer[(y*13)+x] >> 4 & 0x0F;
						rightmost = frameBuffer[(y*13)+x] & 0x0F;
						if (leftmost > 0) leftmost--;
						if (rightmost > 0) rightmost--;
						frameBuffer[(y*13)+x] = (leftmost << 4) | rightmost;
						}
					}
				sendBuffer(&frameBuffer[0]);	// Start transmitting the frameBuffer to Peggy 2.0 over the I2C bus
				delay(100);
				}
			}
			break;

		case 3: {		// Fade in from black
			uint8_t xres = 13;
			uint8_t yres = 25;
			uint8_t leftmost = 0;
			uint8_t rightmost = 0;

			uint8_t pixel;
			uint8_t current_level;

			for(current_level = 0; current_level < 16; current_level++) {
				for (uint8_t y = 0; y < yres; y++) {
					for (uint8_t  x = 0; x < xres; x++) {
						pixel = pgm_read_byte_near(picture + (y*13)+x);
						leftmost = pixel >> 4 & 0x0F;		// NOTE: Should be leftmost
						rightmost = pixel & 0x0F;			// NOTE: should be rightmost

						if (leftmost > current_level) leftmost = current_level;
						if (rightmost > current_level) rightmost = current_level;
						frameBuffer[(y*13)+x] = (leftmost << 4) | rightmost;
					}
				}

				sendBuffer(&frameBuffer[0]); 	// Start transmitting the frameBuffer to Peggy 2.0 over the I2C bus
				delay(100);
				}

			}
			break;
		}
}

/*
	This is the rather RAM wasteful approach to updating the Peggy 2.0 display.
	The function requires a pointer to a uint8_t/char buffer to transmit in one
	big gulp. It should ideally be used as little as possible - it would be much
	better to simply write to the Peggy 2.0 directly without using a full
	frameBuffer every time
*/
void sendBuffer(uint8_t *bufferptr)
{
	uint8_t *ptr = header;
	int count = sizeof(header);

	Wire.beginTransmission(PEGGY_ADDRESS);
	while (count--)
	{
		Wire.write(*ptr++);
	}
	Wire.endTransmission();

	count = 25;

	while (count-- )
	{
		Wire.beginTransmission(PEGGY_ADDRESS);
		Wire.write(bufferptr,13);
		bufferptr+=13;
		Wire.endTransmission();
	}
}


/**
	\brief Processes data (messages) from the XBee via Serial1

	This function receives messages on this format: {s0c1:Alive.mp3}
	Format: s?			= numeric ID of sender (0 = MegaMan, 1 = Wally, 2 = PC)
			c?:param	= command number with argument (max length = 64 chars)
							list of commands:
						0: event notification	: description of event
						1: play MP3 file		: full filename of MP3 file
						2: display text			: text
						3: play animation		: animation name
						4: time					: current time from RTC
						5: alert				: alert type
						9: heartbeat			: heartbeat

	Returns 0 if message is heartbeat, 1 otherwise - NOTE: MAKE THIS SMARTER!

	We ack the messages by retuning "ack:c:param" on the serial line, or if malformed
	message/illegal format: "ERR"

	\return 0==heartbeat, no action required 1==alarm, needs processing
*/
uint8_t processXbeeMsg(void)
{
	uint8_t returnValue;		// Set to zero if we're getting a hearbeat

	boolean started = false;	// Have we received an opening curly bracket?
	boolean ended = false;		// Have we received and closing curly bracket?
	boolean msgOK = true;		// Is the message understandable and syntactically correct?

	char inData[128]; 			// Max length of UART buffer is 128 byte
	byte index = 0;


	while(Serial1.available() > 0)				// We have data waiting for us in the UART buffer
	{
		char inChar = Serial1.read();
		if (!started && inChar !='{') continue;	// Until we have a start message we skip

		if(inChar == '{')						// The beginning of a message
		{
			started = true;
			index = 0;							// We've found start of message - resetting index
			inData[index] = '\0';				// We replace the curly bracket with a zero char
		}
		else if(inChar == '}')					// The end of a message
		{
			ended = true;
			inData[index] = '\0';				// We replace the curly bracket with a zero char
			break;
		}
		else									//
		{
			if(index < 128-1)					// Array size
			{
				inData[index++] = inChar;		// Current character added to array
				inData[index] = '\0';			// We add a zero after current char (string termination)
			}
		}

	}
	// We now have gotten the full message
	if (started && (inData[0] != 's' || inData[3] != 'c' || inData[5] != ':'))	// Malformed message
		{
		Serial.println("Message malformed. Ignoring...");
		Serial1.println("ERR");
		returnValue = 0;	// We do not interrupt what we're doing if we get malformed messages, just alert the sender so he can try again
	} else		// Message ok
	{
		Serial1.println("ACK");
		sender = inData[1] - (65-17);	//Converting form ASCII
		cmd = inData[4] - (65-17);
		cmdParam = &inData[6];			// Points to beginning of argument string

		Serial.print("Sender: ");
		Serial.print(sender);
		Serial.print(", Command: ");
		Serial.print(cmd);
		Serial.print(", Argument: ");

		char *anchor;
		anchor = cmdParam;

		while(*cmdParam != '\0')
			Serial.print(*cmdParam++);
		cmdParam = anchor;

		Serial.println(".");

	}

	// Prepare return value depending on message being a heartbeat or not
	if(cmd == 9) returnValue = 0;	// it's a heartbeat
		else returnValue = 1;		// it's a message we must process
	return returnValue;
}


void setup()
{
	Serial.begin(115200);	// Setting 115200 baud rate for UART for debug messages
	Serial1.begin(9600);	// Setting 115200 baud rate for UART1 for XBee communication
	Serial.println();
	Serial.println(F(" *** I LIVE AGAIN ***"));	// Note! The F() reads from flash and saves RAM
	Serial1.begin(9600);
	Serial1.println(F("{s0:c0:MEGAMAN READY}"));		// report in - note the newline to ease reading on PC


	Serial.print(F("Initializing I2C communication..."));
	Wire.begin();			// Initializing I2C
	pinMode(A4, INPUT);		// set pin A4 to input (for I2C)
	digitalWrite(A4, HIGH);	// turn on pullup resistors on A4 (for I2C)
	pinMode(A5, INPUT);		// set pin A5 to input (for I2C)
	digitalWrite(A5, HIGH);	// turn on pullup resistors on A5 (for I2C)
	// jack up the frequency for I2C (TWI), we need a pretty high rate  from the TWI engine for this to handle 115k input
	// without getting buffer overruns
	TWSR &= ~(1<<TWPS0);
	TWSR &= ~(1<<TWPS1);
	TWBR = ((F_CPU / TWI_FREQ) - 16) / 2;
	Serial.println(TWBR);
	Serial.println(F("DONE"));

	delay(100);

	Serial.print(F("Initializing MP3 Player Shield..."));
	uint8_t result = MP3player.begin();
	if(result != 0)	// Error...	Usually due to misconfigured SdFat library, SdFatConfig.h to be precise
	{
		// 0 OK
		// 1 SD Card Init Failure
		// 2 SD Card File System (FAT) init failure
		// 3 SD Card Root Directory init failure
		// 4 MP3 Decoder mode failure
		// 5 MP3 Decoder speed failure
		/*	My physical wiring of MegaMan involves mapping pin 9 on the Mp3 Player Shield to ping 53 on the Arduino Mega 2560
		*	This was the default for the SFMP3 library before. In later versions this became unnecessary, but I had already made
		*	made the change, which makes all new and unmodified version of the library fail. To fix this add this:
		*	In SFEMP3Shield.cpp: pinMode(53, OUTPUT);  // must be uncommented
		*   In SFEMP3Shield.h: #define SD_SEL 53 // change from 9
		*/
    	Serial.print(F("Error code: "));
    	Serial.print(result);
    	Serial.println(F(" when trying to start MP3 player"));
    	delay(1000); // giving time to send serial error message before halting execution
    	exit(0);
    }
   	MP3player.SetVolume(speakerVolume,speakerVolume);
   	MP3player.playMP3("Hi.mp3");	// Saying hello

    Serial.println(F("DONE"));

    Serial.print(F("Initializing timer..."));

	cli();	// Disable global interrupts
	// The double buffered Output Compare Register (OCR5A and OCR5B) are compared with the
	// Timer/Counter value at all times. The compare match event will also set the
	// Compare Flag (OCF5A or OCF5B) which can be used to generate an Output Compare interrupt request

	TCCR5A = 0x00; // No output - CTC mode (Clear Timer on Compare match)
	TCCR5B = 0x0a; // 0000 1010 - CTC (OCR1A), f/8 prescaler (enabling WGM52 and CS51 bits in the TCCR5B registry)

	// Set OCR5A register to 46875 (0b10110111 00011011)- once every second with 16MHz and clk/8 prescaler
	//                                 ^^OCR5AH  ^^OCR5AL
	OCR5AH = (tics_per_msec >> 8) & 0xff;	// Output Compare Register A, High byte for TIMER5 (0xff = 0b11111111)
	OCR5AL = (tics_per_msec >> 0) & 0xff;	// Output Compare Register A, Low byte for TIMER5
			// NOTE: Do we really need to AND these values???
			// NOTE: Verify that this is actually correct for 16MHz (46875)

	// Interrupt mask register - interrupt on OCR1 match
	TIMSK5 = 0x02;	// Timer/Counter5 Interrupt Mask Register set to clk/8 prescaler (timer runs on 16Mhz/8=2MHz)

	sei();	// Enable global interrupts

	Serial.println(F("DONE"));
	delay(1000);			// Giving the Peggy 2.0 time to get ready
	Serial.println(F("Starting main loop{}"));


	updatePeggyDisplay("waiting");
	updatePeggyDisplay("clear");	// clearing display



}


/**
*	\brief Arduino main loop
*
*	Main loop which
*	Please note: the serial to I2C conversion does NOT touch the frameBuffer. This means that we cannot
*	play with whatever is currently being displayed at the Peggy 2.0 as this is unknown to us
*	This could be circumvented by copying the serial input to the Peggy 2.0 via the frameBuffer (slow!)
*	or we could query the Peggy 2.0 for the current contents of the "screen" (unimplemented on both sides)
*/
void loop()
{


/////////////////////////  TEST AREA //////////////////////////////////
	/*	while(*cmdParam != '\0')
			Serial.print(*cmdParam++);
		Serial.println("-");
*/
/*
p2animScrollText("FIRST");
delay(50);
updatePeggyDisplay("sound");
delay(50);
Serial.println("Scan done");
p2animScrollText("SECOND");
updatePeggyDisplay("waiting");
delay(50);
//Serial.println("Skull done");
//Serial.println("Iteration done");


//p2animScrollText("THIS IS A RATHER LONG SCROLLING TEXT TO SEE WHERE WE CAN PLACE SOME DECORATION");
*/

////////////////////////  END OF TEST AREA ////////////////////////////

	if(abortFlag) {	// if set, we have a message received from the XBee
		//Serial.print("-");
		//while(*cmdParam != '\0')
		//	Serial.print(*cmdParam++);
		//Serial.println("-");

		if (!strcmp(cmdParam, "SCANNING")) {
			updatePeggyDisplay("scan");
		} else if (!strcmp(cmdParam, "SOUND DETECTED")) {
			updatePeggyDisplay("sound");
			delay(1000);
		} else if (!strcmp(cmdParam, "TARGET FOUND")) {
			delay(1000);
			updatePeggyDisplay("skull");
			delay(2000);	// debug debug
			updatePeggyDisplay("clear");	// clearing display
		} else {
			Serial.println("Unknown message, scrolling ");
			p2animScrollText(&cmdParam[0]);
		}
		abortFlag = 0;	// resetting abort flag

	}

}

/// \}
